/***************************************************************************
 创建者: 华磊
 开始时间: 2018.8.14
 copyright: (C) 华友高科
 修改说明: (每次有修改就添加一条，带有 修改人，修改时间，修改描述)
 example (1) hualei 2017.5.8 类的具体实现代码编写

 ***************************************************************************/
#include "safetyfunction.h"
#include "messagelog.h"
#include <qthread.h>
//#include "robotcontrol.h"

#include <qsettings.h>
#include "powermanager.h"
//#include "globalfunction.h"
//#include "../GlobalFunction/globalfunction.h"
#include <qdebug.h>
#include "ultrasonicsensor.h"

#define NSEC_PER_SEC 1000000000

#define D_QUICKSTOP_DELAY 50
#define D_SAFETY_LOOP_PERIOD 80000   //300000检测按钮有点慢了
#define D_POWER_LOSE_SHUTDOWN_WAIT_TIME 100
#define D_POWER_LOSE_MESSAGE_ID 9039

//#define D_EMSTOP1_IND 0
//#define D_EMSTOP2_IND 1
#define D_SERVOON_IND 2
#define D_RESET_ALARM_IND 3
#define D_STOP_IND 4
#define D_CEASE_IND 5
#define D_RUN_IND 6
#define D_EXT_SERV_ON_IND 7
#define D_ENABLE_RUN1_IND 8
#define D_ENABLE_RUN2_IND 9
#define D_ENABLE_RUN3_IND 10
#define D_ENABLE_RUN4_IND 11
#define D_SLOW_DOWN1_IND 12
#define D_SLOW_DOWN2_IND 13
#define D_SLOW_DOWN3_IND 14
#define D_SLOW_DOWN4_IND 15

#define D_ERROR_STATUS_IND 0
#define D_CEASE_STATUS_IND 1
#define D_RUN_STATUS_IND 2
#define D_STOP_STATUS_IND 3
#define D_SYSTEM_READY_IND 4
#define D_AGV_RED_LED_IND 5
#define D_AGV_GREEN_LED_IND 6
#define D_AGV_BLUE_LED_IND 7


//TODO 1)AUTO MODE 2)OPEN FILE

extern PowerManager *p_powerManger;
extern UltrasonicSensor *g_ultrasonicSensor;

SafetyFunction::SafetyFunction()
{
    motorBatteryCheckCount=0;
    safetyCheckFlag=false;
    emergencyStopStatus=false;
    powerCheckFlag=0;
    isErrorStatus=false;
    isPlayErrorMusic=false;
    isPlayBarrierMusic=false;
    laser1MonitorFlag=1;
    laser2MonitorFlag=1;
    isInitialOkStatus=false;
    isStartUpServoOnFinished=false;

    slowDownFlag.resize(D_SLOWDOWN_SIZE);
    slowDownRatio.resize(D_SLOWDOWN_SIZE);
    slowDownTime.resize(D_SLOWDOWN_SIZE);
    isSlowDownState.resize(D_SLOWDOWN_SIZE);
    diOld_slow.resize(D_SLOWDOWN_SIZE);
    for(int i=0;i<D_SLOWDOWN_SIZE;i++)
    {
        slowDownFlag[i]=0;
        slowDownRatio[i]=0.1;
        slowDownTime[i]=1;
        diOld_slow[i]=false;
        isSlowDownState[i]=false;
    }


    initialedEnableRun1=false;
    initialedEnableRun2=false;
    initialedEnableRun3=false;
    initialedEnableRun4=false;

    enableRun1Flag=0;
    enableRun2Flag=0;
    enableRun3Flag=0;
    enableRun4Flag=0;
    resetAlarmFlag=0;
    extServOnFlag=0;
    stopFlag=0;
    ceaseFlag=0;
    runFlag=0;
    emergencyStopMonitorFlag=1;
    hmiServoDiMonitorFlag=1;
    autoStartUpServOnFlag=0;
    ultrasonicCheckFlag=0;

    ultrasonicSensor=NULL;

    configFilePath=D_TCRCONTROLLER_DATA_PATH;
    configFilePath+=SAFETY_SETTING_FILE_PATH;
    if(1!=readSettingFile(configFilePath))
    {
        addMsg(ENUM_MSG_ERROR,"SafetyFunction","SafetyFunction",3720,-1);
    }
    else
    {
        isInitialOkStatus=true;
    }
    if(1==ultrasonicCheckFlag)
    {
        QString ultrasonicConfigFilePath=D_TCRCONTROLLER_DATA_PATH;
        ultrasonicConfigFilePath+=ULTROSONIC_SENSOR_FILE_PATH;
        ultrasonicSensor=new UltrasonicSensor(ultrasonicConfigFilePath);
        g_ultrasonicSensor=ultrasonicSensor;
        if(false==ultrasonicSensor->isInitialOkStatus())
        {
            qDebug()<<"error, ultrasonic sensor error";
            addMsg(ENUM_MSG_ERROR,"SafetyFunction","SafetyFunction",3715,-1);

        }
    }
    createThread();
}

int SafetyFunction::setSefetyCheck(E_SAFETY_CHECK typeIn, int isEnable)
{
    qDebug()<<"setSefetyCheck,typeIn"<<typeIn<<"isEnable"<<isEnable;
    QSettings settings(configFilePath, QSettings::IniFormat);

    switch (typeIn) {
    case E_SAFETY_CHECK_emergencyStopMonitorFlag:
        emergencyStopMonitorFlag=isEnable;
        settings.setValue("SystemInfo/emergencyStopMonitorFlag",QVariant::fromValue(isEnable));
        break;
    case E_SAFETY_CHECK_isEnableRun_autoMode:
        isEnableRun_autoMode=isEnable;
        settings.setValue("SystemInfo/isEnableRun_autoMode",QVariant::fromValue(isEnable));
        break;
    case E_SAFETY_CHECK_isPlayErrorMusic:
        isPlayErrorMusic=isEnable;
        settings.setValue("SystemInfo/isPlayErrorMusic",QVariant::fromValue(isEnable));
        break;
    case E_SAFETY_CHECK_isPlayBarrierMusic:
        isPlayBarrierMusic=isEnable;
        settings.setValue("SystemInfo/isPlayBarrierMusic",QVariant::fromValue(isEnable));
        break;
    case E_SAFETY_CHECK_runFlag:
        runFlag=isEnable;
        settings.setValue("SystemInfo/runFlag",QVariant::fromValue(isEnable));
        break;
    case E_SAFETY_CHECK_autoStartUpServOnFlag:
        isEnableRun_autoMode=isEnable;
        settings.setValue("SystemInfo/isEnableRun_autoMode",QVariant::fromValue(isEnable));
        break;
    case E_SAFETY_CHECK_runSafetyCheck1Flag:
        runSafetyCheck1Flag=isEnable;
        settings.setValue("SystemInfo/runSafetyCheck1Flag",QVariant::fromValue(isEnable));
        break;
    case E_SAFETY_CHECK_runSafetyCheck2Flag:
        runSafetyCheck2Flag=isEnable;
        settings.setValue("SystemInfo/runSafetyCheck2Flag",QVariant::fromValue(isEnable));
        break;
    case E_SAFETY_CHECK_runSafetyCheck3Flag:
        runSafetyCheck3Flag=isEnable;
        settings.setValue("SystemInfo/runSafetyCheck3Flag",QVariant::fromValue(isEnable));
        break;
    case E_SAFETY_CHECK_runSafetyCheck4Flag:
        runSafetyCheck4Flag=isEnable;
        settings.setValue("SystemInfo/runSafetyCheck4Flag",QVariant::fromValue(isEnable));
        break;
    case E_SAFETY_CHECK_ultrasonicCheckFlag:
        ultrasonicCheckFlag=isEnable;
        settings.setValue("SystemInfo/ultrasonicCheckFlag",QVariant::fromValue(isEnable));
        break;
    case E_SAFETY_CHECK_ceaseFlag:
        ceaseFlag=isEnable;
        settings.setValue("SystemInfo/ceaseFlag",QVariant::fromValue(isEnable));
        break;
    case E_SAFETY_CHECK_stopFlag:
        stopFlag=isEnable;
        settings.setValue("SystemInfo/stopFlag",QVariant::fromValue(isEnable));
        break;
    case E_SAFETY_CHECK_extServOnFlag:
        extServOnFlag=isEnable;
        settings.setValue("SystemInfo/extServOnFlag",QVariant::fromValue(isEnable));
        break;
    case E_SAFETY_CHECK_resetAlarmFlag:
        resetAlarmFlag=isEnable;
        settings.setValue("SystemInfo/resetAlarmFlag",QVariant::fromValue(isEnable));
        break;
    case E_SAFETY_CHECK_enableRun4Flag:
        enableRun4Flag=isEnable;
        settings.setValue("SystemInfo/enableRun4Flag",QVariant::fromValue(isEnable));
        break;
    case E_SAFETY_CHECK_enableRun3Flag:
        enableRun3Flag=isEnable;
        settings.setValue("SystemInfo/enableRun3Flag",QVariant::fromValue(isEnable));
        break;
    case E_SAFETY_CHECK_enableRun2Flag:
        enableRun2Flag=isEnable;
        settings.setValue("SystemInfo/enableRun2Flag",QVariant::fromValue(isEnable));
        break;
    case E_SAFETY_CHECK_enableRun1Flag:
        enableRun1Flag=isEnable;
        settings.setValue("SystemInfo/enableRun1Flag",QVariant::fromValue(isEnable));
        break;
    case E_SAFETY_CHECK_slowDownFlag1:
        slowDownFlag[0]=isEnable;
        settings.setValue("SystemInfo/slowDownFlag1",QVariant::fromValue(isEnable));
        break;
    case E_SAFETY_CHECK_slowDownFlag2:
        slowDownFlag[1]=isEnable;
        settings.setValue("SystemInfo/slowDownFlag2",QVariant::fromValue(isEnable));
        break;
    case E_SAFETY_CHECK_slowDownFlag3:
        slowDownFlag[2]=isEnable;
        settings.setValue("SystemInfo/slowDownFlag3",QVariant::fromValue(isEnable));
        break;
    case E_SAFETY_CHECK_slowDownFlag4:
        slowDownFlag[3]=isEnable;
        settings.setValue("SystemInfo/slowDownFlag4",QVariant::fromValue(isEnable));
        break;
    case E_SAFETY_CHECK_powerCheckFlag:
        powerCheckFlag=isEnable;
        settings.setValue("SystemInfo/powerCheckFlag",QVariant::fromValue(isEnable));
        break;
    case E_SAFETY_CHECK_hmiServoDiMonitorFlag:
        hmiServoDiMonitorFlag=isEnable;
        settings.setValue("SystemInfo/hmiServoDiMonitorFlag",QVariant::fromValue(isEnable));
        break;


    default:
        qDebug()<<"error ,unknow type"<<typeIn;
        return -1;
        break;
    }
    return 1;
}

int SafetyFunction::getSefetyCheck(E_SAFETY_CHECK typeIn, int &checkStatusReturn)
{
    switch (typeIn) {
    case E_SAFETY_CHECK_emergencyStopMonitorFlag:
        checkStatusReturn=emergencyStopMonitorFlag;
        break;
    case E_SAFETY_CHECK_isEnableRun_autoMode:
        checkStatusReturn=isEnableRun_autoMode;
        break;
    case E_SAFETY_CHECK_isPlayErrorMusic:
        checkStatusReturn=isPlayErrorMusic;
        break;
    case E_SAFETY_CHECK_isPlayBarrierMusic:
        checkStatusReturn=isPlayBarrierMusic;
        break;
    case E_SAFETY_CHECK_runFlag:
        checkStatusReturn=runFlag;
        break;
    case E_SAFETY_CHECK_autoStartUpServOnFlag:
        checkStatusReturn=autoStartUpServOnFlag;
        break;
    case E_SAFETY_CHECK_runSafetyCheck1Flag:
        checkStatusReturn=runSafetyCheck1Flag;
        break;
    case E_SAFETY_CHECK_runSafetyCheck2Flag:
        checkStatusReturn=runSafetyCheck2Flag;
        break;
    case E_SAFETY_CHECK_runSafetyCheck3Flag:
        checkStatusReturn=runSafetyCheck3Flag;
        break;
    case E_SAFETY_CHECK_runSafetyCheck4Flag:
        checkStatusReturn=runSafetyCheck4Flag;
        break;
    case E_SAFETY_CHECK_ultrasonicCheckFlag:
        checkStatusReturn=ultrasonicCheckFlag;
        break;
    case E_SAFETY_CHECK_ceaseFlag:
        checkStatusReturn=ceaseFlag;
        break;
    case E_SAFETY_CHECK_stopFlag:
        checkStatusReturn=stopFlag;
        break;
    case E_SAFETY_CHECK_extServOnFlag:
        checkStatusReturn=extServOnFlag;
        break;
    case E_SAFETY_CHECK_resetAlarmFlag:
        checkStatusReturn=resetAlarmFlag;
        break;
    case E_SAFETY_CHECK_enableRun4Flag:
        checkStatusReturn=enableRun4Flag;
        break;
    case E_SAFETY_CHECK_enableRun3Flag:
        checkStatusReturn=enableRun3Flag;
        break;
    case E_SAFETY_CHECK_enableRun2Flag:
        checkStatusReturn=enableRun2Flag;
        break;
    case E_SAFETY_CHECK_enableRun1Flag:
        checkStatusReturn=enableRun1Flag;
        break;
    case E_SAFETY_CHECK_slowDownFlag1:
        checkStatusReturn=slowDownFlag[0];
        break;
    case E_SAFETY_CHECK_slowDownFlag2:
        checkStatusReturn=slowDownFlag[1];
        break;
    case E_SAFETY_CHECK_slowDownFlag3:
        checkStatusReturn=slowDownFlag[2];
        break;
    case E_SAFETY_CHECK_slowDownFlag4:
        checkStatusReturn=slowDownFlag[3];
        break;
    case E_SAFETY_CHECK_powerCheckFlag:
        checkStatusReturn=powerCheckFlag;
        break;
    case E_SAFETY_CHECK_hmiServoDiMonitorFlag:
        checkStatusReturn=hmiServoDiMonitorFlag;
        break;


    default:
        qDebug()<<"error ,unknow type"<<typeIn;
        return -1;
        break;
    }
    return 1;
}

int SafetyFunction::setUltrasonicSensorDebugFlag(int debugFlagIn)
{
    if(NULL==ultrasonicSensor)
    {
        return -1;
    }
    return ultrasonicSensor->setDebugFlag(debugFlagIn);
}

int SafetyFunction::getUltrasonicSensorName(std::vector<string> &sensorNames)
{
    if(NULL==ultrasonicSensor)
    {
        return -1;
    }
    return ultrasonicSensor->getSensorName(sensorNames);
}

int SafetyFunction::setUltrasonicSensor485Address(int origineAddressIn, int addressIn)
{
    if(NULL==ultrasonicSensor)
    {
        return -1;
    }
    return ultrasonicSensor->setSensor485Address(origineAddressIn,addressIn);
}

int SafetyFunction::setUltrasonicSensorCheckEnable(bool isOn)
{
    if(NULL==ultrasonicSensor)
    {
        return -1;
    }
    return ultrasonicSensor->setUltrasonicSensorCheckEnable(isOn);
}

int SafetyFunction::getUltrasonicSensorSafeStatus(std::vector<double> &safeStatusOut, bool &isCheckEnableOut)
{
    if(NULL==ultrasonicSensor)
    {
        return -1;
    }
    return ultrasonicSensor->getUltrasonicSensorSafeStatus(safeStatusOut,isCheckEnableOut);
}

int SafetyFunction::setUltrasonicPowerSupplyNoiseClass(int classIn)
{
    if(NULL==ultrasonicSensor)
    {
        return -1;
    }
    return ultrasonicSensor->setUltrasonicPowerSupplyNoiseClass((EM_POWER_NOISE_CLASS)classIn);
}

int SafetyFunction::setUltrasonicWork(int locationIn, bool isEnable)
{
    if(NULL==ultrasonicSensor)
    {
        return -1;
    }
    return ultrasonicSensor->setSensorWork(locationIn,isEnable);
}

int SafetyFunction::getUltrasonicDetectDistance(QVector<double> &distanceResultOut)
{
    if(NULL==ultrasonicSensor)
    {
        return -1;
    }
    return ultrasonicSensor->getDetectDistanceFromBuffer(distanceResultOut);
}

int SafetyFunction::getUltrasonicDetectDistance(int locationIn, QVector<double> &distanceResultOut)
{
    if(NULL==ultrasonicSensor)
    {
        return -1;
    }
    return ultrasonicSensor->getDetectDistanceFromBuffer(locationIn,distanceResultOut);
}

int SafetyFunction::getUltrasonicStatus(QVector<int>  &deviceRequestCountOut)
{
    if(NULL==ultrasonicSensor)
    {
        return -1;
    }
    return ultrasonicSensor->getSensorStatus(deviceRequestCountOut);
}

int SafetyFunction::ultrasonicCheck()
{
    if(1!=ultrasonicCheckFlag )
    {
        return 1;
    }
    int errorIdOut;

    if(1!=ultrasonicSensor->isAllSensorRequestResponseOk(errorIdOut))
    {
        //qDebug()<<"SafetyFunction::ultrasonicCheck() －－－－－－－－－－－－－－－－－－－";
        addMsg(ENUM_MSG_ERROR,"SafetyFunction","SafetyFunction",3718,-1,errorIdOut);

    }

    return 1;

}

int SafetyFunction::motorBatteryCheck()
{

    if( (60000/D_SAFETY_LOOP_PERIOD) < motorBatteryCheckCount)//60S
    {
        return -1;
    }
    motorBatteryCheckCount++;
    static bool firstCheck=true;
    if(  true==p_powerManger->isEthercatDeviceInitialOk() && true==firstCheck)
    {
        p_powerManger->motorBatteryCheck();
        qDebug()<<"SafetyFunction::motorBatteryCheck(),20000/D_SAFETY_LOOP_PERIOD";
        firstCheck=false;
    }
    else if( (60000/D_SAFETY_LOOP_PERIOD) == motorBatteryCheckCount)//60S
    {
        p_powerManger->motorBatteryCheck();
        qDebug()<<"SafetyFunction::motorBatteryCheck(),60000/D_SAFETY_LOOP_PERIOD";
    }
    return 1;

}

int SafetyFunction::safetyCheckLoop()
{

    if(false==isInitialOkStatus)
    {
        addMsg(ENUM_MSG_ERROR,"SafetyFunction","SafetyFunction",3721,-1);
    }
    emergencyStopDiMonitor();
    laserMonitor();
    hmiServoOnDiMonitor();

//    motorServoOnMonitor();
    errorMessageMonitor();
    powerLoseCheck();
    motorBatteryCheck();
    systemDoOutput();
    systemDiMonitor();
    autoStartUpServoOn();
    ultrasonicCheck();

    systemInitialCheck();

    return 1;
}

//int SafetyFunction::motorServoOnMonitor()
//{
//    if(true==emergencyStopStatus && true==p_powerManger->isRobotServoOn(-1))
//    {
//        p_powerManger->stopRobotEngine(-1);
//        qDebug()<<"motorServoOnMonitor, true==emergencyStopStatus && true==isRobotServoOn(-1)";
//        addMsg(ENUM_MSG_ERROR,"SafetyFunction","SafetyFunction",3701,-1);
//    }
//    return 1;
//}

int SafetyFunction::powerLoseCheck()
{
    if(1!=powerCheckFlag)
    {
        return 1;
    }
    QVector<Message> messageList;
    MessageLog::getInstance()->getAllMessage(messageList);

    //判断３9号ethercat网络断开消息
    bool powerLoseFlag=false;
    for(int i=0;i<messageList.size();i++)
    {
        if(D_POWER_LOSE_MESSAGE_ID==messageList[i].messageCode)
        {
            powerLoseFlag=true;
            //qDebug()<<"SafetyFunction::powerLoseCheck() powerLoseFlag=true－－－－－－－－－－－－－－－－－－－";
            addMsg(ENUM_MSG_ERROR,"SafetyFunction","SafetyFunction",3703,-1,D_POWER_LOSE_SHUTDOWN_WAIT_TIME);
            break;
        }
    }

    if(true==powerLoseFlag)
    {
       sleep(D_POWER_LOSE_SHUTDOWN_WAIT_TIME);
       MessageLog::getInstance()->getAllMessage(messageList);
       for(int i=0;i<messageList.size();i++)
       {
           if(D_POWER_LOSE_MESSAGE_ID==messageList[i].messageCode)
           {
               p_powerManger->controllerShutDown();
               return -1;
           }
       }
    }

    return 1;
}

int SafetyFunction::startSafetyCheck()
{
    safetyCheckFlag=true;
    return 1;
}

int SafetyFunction::stopSafetyCheck()
{
    safetyCheckFlag=false;
    return 1;
}

//int SafetyFunction::resetEmergencyStop()
//{
//    emergencyStopStatus=false;
//    return 1;
//}

int SafetyFunction::emergencyStopDiMonitor()
{
    if(0==emergencyStopMonitorFlag || false==p_powerManger->isEthercatDeviceInitialOk())
    {
        return 1;
    }

    bool emstop1, emstop2;
    emstop1=false;
    emstop2=false;
    if(-100>=p_powerManger->getEmergencyDi(emstop1,emstop2))
    {
        qDebug()<<"没有获取到急停开关信息,p_powerManger->getEmergencyDi(emstop1,emstop2)";
        addMsg(ENUM_MSG_ERROR,"SafetyFunction","SafetyFunction",3714,-1);
        return 1;
    }
    else
    {
        bool isStop=false;
        if(1==emergencyStopMonitorFlag && false==emstop1)
        {
            isStop=true;
        }
        if(2==emergencyStopMonitorFlag && (false==emstop1||false==emstop2))
        {

            isStop=true;
        }
        static int servoOffDelay=D_QUICKSTOP_DELAY;
        if(true==isStop)
        {
            if(servoOffDelay>0)
            {
                servoOffDelay--;
            }

            emergencyStopStatus=true;
//            p_powerManger->stopRobotEngine(-1);
            int returnFlag;
            p_powerManger->quickStop(userInfo,returnFlag);
            if(0==servoOffDelay)
            {
                  p_powerManger->servoOffAllRobot();
            }
            addMsg(ENUM_MSG_ERROR,"SafetyFunction","SafetyFunction",3700,-1);
        }
        else
        {
            servoOffDelay=0;
        }
        return -1;

    }
    return 1;
}

int SafetyFunction::laserMonitor()
{
    int returnFlag;
    if(laser1MonitorFlag>=0)
    {

        if(laserOkValue!=p_powerManger->getSystemDiStatus(laser1MonitorFlag))
        {
            p_powerManger->quickStop(userInfo,returnFlag);
            addMsg(ENUM_MSG_ERROR,"SafetyFunction","SafetyFunction",3719,-1,1);
        }

    }

    if(laser2MonitorFlag>=0)
    {

        if(laserOkValue!=p_powerManger->getSystemDiStatus(laser2MonitorFlag))
        {
            p_powerManger->quickStop(userInfo,returnFlag);
            addMsg(ENUM_MSG_ERROR,"SafetyFunction","SafetyFunction",3719,-1,2);

        }

    }


    return 1;
}

int SafetyFunction::errorMessageMonitor()
{
    static int servoOffDelay=D_QUICKSTOP_DELAY;
    static bool isErrorStatus_old=false;
    if(true==MessageLog::getInstance()->isErrorOccured())
    {
        if(servoOffDelay>0)
        {
            servoOffDelay--;
        }
        isErrorStatus=true;
        p_powerManger->setSystemDo(D_ERROR_STATUS_IND,true);
        p_powerManger->setSystemDo(D_AGV_RED_LED_IND,true);
        p_powerManger->setSystemDo(D_AGV_GREEN_LED_IND,false);
        p_powerManger->setSystemDo(D_AGV_BLUE_LED_IND,false);
        if(ENUM_COMMAND_RUN==p_powerManger->getCurrentRobotBasicStatus())
        {
//           p_powerManger->stopRobotEngine(-1);
            int returnFlag;
            p_powerManger->quickStop(userInfo,returnFlag);
            if(0==servoOffDelay)
            {
                  p_powerManger->servoOffAllRobot();
            }

            addMsg(ENUM_MSG_ERROR,"SafetyFunction","SafetyFunction",3702,-1);
        }
        else
        {
            int returnFlag;
            p_powerManger->quickStop(userInfo,returnFlag);
//            p_powerManger->programAllReset(userInfo,returnFlag);//导致行号归零
            if(0==servoOffDelay)
            {
                  p_powerManger->servoOffAllRobot();//导致手动超过限位时，算法一直报没有使能
            }

            addMsg(ENUM_MSG_ERROR,"SafetyFunction","SafetyFunction",3717,-1);
        }
        if(false==isErrorStatus_old && true==isPlayErrorMusic)
        {
           p_powerManger->playErrorMusic();
        }
        p_powerManger->errorNoticeForFleetService();
    }
    else
    {
        servoOffDelay=D_QUICKSTOP_DELAY;
        isErrorStatus=false;
        p_powerManger->setSystemDo(D_ERROR_STATUS_IND,false);
        p_powerManger->setSystemDo(D_AGV_RED_LED_IND,false);
        if(true==isErrorStatus_old && true==isPlayErrorMusic)
        {
           p_powerManger->restoreNormalMusic();
        }
    }
    isErrorStatus_old=isErrorStatus;
    return 1;
}

int SafetyFunction::hmiServoOnDiMonitor()
{
    if(1!=hmiServoDiMonitorFlag)
    {
        return 1;
    }

    static bool diOld=false;
    bool diNew;
    p_powerManger->getServoOnDi(diNew);
    if(false==diOld && true==diNew)
    {
        p_powerManger->servoOnCurrentRobot();

    }
    else if(true==diOld && false==diNew)
    {
        p_powerManger->servoOffCurrentRobot();
    }
    diOld=diNew;

    return 1;
}

int SafetyFunction::systemInitialCheck()
{
    static bool isPlayed=false;
    if(false==isPlayed)
    {
        if(true==MessageLog::getInstance()->isDeviceMasterInitialOk())
        {
            qDebug()<<"SafetyFunction::systemInitialCheck playSystemMusic systemInitialFinished.mp3";
            p_powerManger->playSystemMusic("systemInitialFinished.mp3",1);
            isPlayed=true;
        }
    }

}

int SafetyFunction::autoStartUpServoOn()
{
    if(1!=autoStartUpServOnFlag || false==p_powerManger->isEthercatDeviceInitialOk()
            || false==p_powerManger->isCurrentRobotAxisZeroOk() ||isStartUpServoOnFinished)
    {
        return 1;
    }
    int tmpKey;
    int tmpTryCount=3;
    do
    {
//        sleep(1);//校零后，会自动同步反馈和命令，不用等待calibrateReset
        tmpKey=p_powerManger->servoOnCurrentRobot();
        qDebug()<<"-----------------------------------auto serv on,tmpKey"<<tmpKey;
        tmpTryCount--;
    }while(1!=tmpKey && tmpTryCount>0);
    if(1!=tmpKey)
    {
        addMsg(ENUM_MSG_WARNING, "MotionServer", "TrajectoryServer", 3722,-1);
    }

    isStartUpServoOnFinished=true;

    return 1;
}


int SafetyFunction::systemDoOutput()
{
    int tmpStatus=p_powerManger->getCurrentRobotBasicStatus();
    double tmpBarrierRatio;
    static double tmpBarrierRatio_old=1;
//       p_powerManger->getVehicleSafeSpeedRatio(userInfo,tmpRatio);
    p_powerManger->getBarrierProtectRatioOfMoving(userInfo,tmpBarrierRatio);
    //如果有障碍物，则蓝灯亮
    if(tmpBarrierRatio<1.0)
    {
        if(false==isErrorStatus)
        {
            p_powerManger->setSystemDo(D_AGV_BLUE_LED_IND,true);
        }
        if(tmpBarrierRatio!=tmpBarrierRatio_old && true==isPlayBarrierMusic)
        {
            p_powerManger->playBarriorMusic(tmpBarrierRatio);
        }
    }
    else
    {
        p_powerManger->setSystemDo(D_AGV_BLUE_LED_IND,false);
        if(1>tmpBarrierRatio_old  && true==isPlayBarrierMusic)
        {
            p_powerManger->restoreNormalMusic();
        }
    }
    tmpBarrierRatio_old=tmpBarrierRatio;

    if(ENUM_COMMAND_RUN==tmpStatus)
    {
       p_powerManger->setSystemDo(D_CEASE_STATUS_IND,false);
       p_powerManger->setSystemDo(D_RUN_STATUS_IND,true);
       p_powerManger->setSystemDo(D_STOP_STATUS_IND,false);

       p_powerManger->setSystemDo(D_AGV_GREEN_LED_IND,true);

    }
    else if(ENUM_COMMAND_CEASE==tmpStatus)
    {
       p_powerManger->setSystemDo(D_CEASE_STATUS_IND,true);
       p_powerManger->setSystemDo(D_RUN_STATUS_IND,false);
       p_powerManger->setSystemDo(D_STOP_STATUS_IND,false);
       if(false==isErrorStatus)
       {
           p_powerManger->setSystemDo(D_AGV_BLUE_LED_IND,true);
       }

       p_powerManger->setSystemDo(D_AGV_GREEN_LED_IND,false);
    }
    else if(ENUM_COMMAND_RESET==tmpStatus)
    {
       p_powerManger->setSystemDo(D_CEASE_STATUS_IND,false);
       p_powerManger->setSystemDo(D_RUN_STATUS_IND,false);
       p_powerManger->setSystemDo(D_STOP_STATUS_IND,true);

       p_powerManger->setSystemDo(D_AGV_BLUE_LED_IND,false);
       p_powerManger->setSystemDo(D_AGV_GREEN_LED_IND,false);
    }
    else
    {
        p_powerManger->setSystemDo(D_CEASE_STATUS_IND,false);
        p_powerManger->setSystemDo(D_RUN_STATUS_IND,false);
        p_powerManger->setSystemDo(D_STOP_STATUS_IND,false);
    }


    bool isReady=p_powerManger->isEthercatDeviceInitialOk();
    if(true==isReady)
    {
       p_powerManger->setSystemDo(D_SYSTEM_READY_IND,true);
    }
    else
    {
        p_powerManger->setSystemDo(D_SYSTEM_READY_IND,false);
    }

    return 1;
}

int SafetyFunction::systemDiMonitor()
{
    UserInfo userInfo;
    int returnFlag;

    enableRunDiMonitor();
    slowDownDiMonitor();

    //extServOnFlag
    if(1==extServOnFlag)
    {
        static bool diOld=false;
        bool diNew;
        diNew=p_powerManger->getSystemDiStatus(D_EXT_SERV_ON_IND);
        if(false==diOld && true==diNew)
        {
//            p_powerManger->servoOnCurrentRobot();
            p_powerManger->servoOnAllRobot();
            qDebug()<<"SafetyFunction::systemDiMonitor, ext di servoOnCurrentRobot";
            addMsg(ENUM_MSG_REMIND,"SafetyFunction","SafetyFunction",3708,-1);
        }
        else if(true==diOld && false==diNew)
        {
//            p_powerManger->servoOffCurrentRobot();
            p_powerManger->servoOffAllRobot();
            qDebug()<<"SafetyFunction::systemDiMonitor, ext di servoOffCurrentRobot";
            addMsg(ENUM_MSG_REMIND,"SafetyFunction","SafetyFunction",3709,-1);
        }
        diOld=diNew;
    }
    //reset alarm
    if(1==resetAlarmFlag)
    {
        static bool diOld_reset=false;
        bool diNew_reset;
        diNew_reset=p_powerManger->getSystemDiStatus(D_RESET_ALARM_IND);
        if(false==diOld_reset && true==diNew_reset)
        {
            p_powerManger->setAllMsgClear(userInfo,returnFlag);
            qDebug()<<"SafetyFunction::systemDiMonitor, ext di reset alarm";
            addMsg(ENUM_MSG_REMIND,"SafetyFunction","SafetyFunction",3710,-1);

        }
        diOld_reset=diNew_reset;
    }

    //stop
    if(1==stopFlag)
    {
        static bool diOld_stop=false;
        bool diNew_stop;
        diNew_stop=p_powerManger->getSystemDiStatus(D_STOP_IND);
        if(false==diOld_stop && true==diNew_stop)
        {
//            p_powerManger->programReset(userInfo,returnFlag);
            p_powerManger->programAllReset(userInfo,returnFlag);
            qDebug()<<"SafetyFunction::systemDiMonitor, ext di stop";
            addMsg(ENUM_MSG_REMIND,"SafetyFunction","SafetyFunction",3711,-1);

        }
        diOld_stop=diNew_stop;
    }

    //cease
    if(1==ceaseFlag)
    {
        static bool diOld_cease=false;
        bool diNew_cease;
        diNew_cease=p_powerManger->getSystemDiStatus(D_CEASE_IND);
        if(false==diOld_cease && true==diNew_cease)
        {
//            p_powerManger->programCease(userInfo,returnFlag);
            //判断调度模式
            if(ENUM_CONTROL_FLEET==p_powerManger->getRunModel())
            {
                p_powerManger->motion_allCease();
                qDebug()<<"SafetyFunction::systemDiMonitor, ext di cease,ENUM_CONTROL_FLEET";
                addMsg(ENUM_MSG_REMIND,"SafetyFunction","SafetyFunction",3723,-1);
            }
            else
            {
                p_powerManger->programAllCease(userInfo,returnFlag);
                qDebug()<<"SafetyFunction::systemDiMonitor, ext di cease";
                addMsg(ENUM_MSG_REMIND,"SafetyFunction","SafetyFunction",3712,-1);
            }



        }
        diOld_cease=diNew_cease;
    }

    //run
    if(1==runFlag && (ENUM_CONTROL_EXT==p_powerManger->getRunModel() || 1==isEnableRun_autoMode))
    {
        static bool diOld_run=false;
        bool diNew_run;
        diNew_run=p_powerManger->getSystemDiStatus(D_RUN_IND);
        if(false==diOld_run && true==diNew_run)
        {
//            p_powerManger->programRun(userInfo,returnFlag);
            //
            //判断调度模式
            if(ENUM_CONTROL_FLEET==p_powerManger->getRunModel())
            {
                p_powerManger->motion_allStartRun();
                p_powerManger->setEngineStatus_fleet(ENUM_COMMAND_RUN);
                qDebug()<<"SafetyFunction::systemDiMonitor, ext di run,ENUM_CONTROL_FLEET";
                addMsg(ENUM_MSG_REMIND,"SafetyFunction","SafetyFunction",3724,-1);
            }
            else
            {
                p_powerManger->setAllMsgClear(userInfo,returnFlag);
                if(false==p_powerManger->isCurrentRobotServoOn())
                {
                    p_powerManger->servoOnCurrentRobot();
                    sleep(2);
                }
                p_powerManger->programAllRun(userInfo,returnFlag);

                qDebug()<<"SafetyFunction::systemDiMonitor, ext di run";
                addMsg(ENUM_MSG_REMIND,"SafetyFunction","SafetyFunction",3713,-1);
            }


        }
        diOld_run=diNew_run;
    }




    return 1;
}

int SafetyFunction::slowDownDiMonitor()
{
    //slowDown
    QVector<bool> diNew_slow;
    diNew_slow.resize(D_SLOWDOWN_SIZE);
    for(int i=0;i<D_SLOWDOWN_SIZE;i++)
    {
        if(1==slowDownFlag[i])
        {
            diNew_slow[i]=p_powerManger->getSystemDiStatus(D_SLOW_DOWN1_IND+i);
            if(false==diOld_slow[i] && true==diNew_slow[i])
            {
                isSlowDownState[i]=true;
//                p_powerManger->setSafeSpeedRatio(slowDownRatio[i],slowDownTime[i]);
                p_powerManger->setAllSafeSpeedRatio(slowDownRatio[i],slowDownTime[i]);
                qDebug()<<"SafetyFunction::systemDiMonitor, ext di slow down in,slowDownRatio="
                       <<slowDownRatio[i]<<" slowDownTime="<<slowDownTime[i];
                addMsg(ENUM_MSG_REMIND,"SafetyFunction","SafetyFunction",3706,-1,
                       (slowDownRatio[i]*100.0),(slowDownTime[i]*1000.0),i+1);

            }
            else if(true==diOld_slow[i] && false==diNew_slow[i])
            {
                isSlowDownState[i]=false;
//                p_powerManger->setSafeSpeedRatio(1,slowDownTime[i]);
                p_powerManger->setAllSafeSpeedRatio(1,slowDownTime[i]);
                qDebug()<<"SafetyFunction::systemDiMonitor, ext di slow down out,slowDownRatio="
                       <<slowDownRatio[i]<<" slowDownTime="<<slowDownTime[i];
                addMsg(ENUM_MSG_REMIND,"SafetyFunction","SafetyFunction",3707,-1,
                       100.0,(slowDownTime[i]*1000.0),i+1);
            }
            diOld_slow[i]=diNew_slow[i];
        }
    }
}

int SafetyFunction::enableRunDiMonitor()
{

    UserInfo userInfo;
    int returnFlag;

    //enableRun1
    if(1==enableRun1Flag)
    {
        static bool diOld_enableRun1=false;
        bool diNew_enableRun1;
        diNew_enableRun1=p_powerManger->getSystemDiStatus(D_ENABLE_RUN1_IND);
        if(false==diOld_enableRun1 && true==diNew_enableRun1 || false==initialedEnableRun1)
        {
            initialedEnableRun1=true;
            p_powerManger->setEnableRun(0,true);
            qDebug()<<"SafetyFunction::systemDiMonitor, ext di enable run1=enable";
            addMsg(ENUM_MSG_REMIND,"SafetyFunction","SafetyFunction",3704,-1,1);

        }
        else if(true==diOld_enableRun1 && false==diNew_enableRun1)
        {
            p_powerManger->programCease(userInfo,returnFlag);
            p_powerManger->setEnableRun(0,false);
            qDebug()<<"SafetyFunction::systemDiMonitor, ext di enable run1=disable";
            addMsg(ENUM_MSG_REMIND,"SafetyFunction","SafetyFunction",3705,-1,1);
        }
        diOld_enableRun1=diNew_enableRun1;
    }

    //enableRun2
    if(1==enableRun2Flag)
    {
        static bool diOld_enableRun2=false;
        bool diNew_enableRun2;
        diNew_enableRun2=p_powerManger->getSystemDiStatus(D_ENABLE_RUN2_IND);
        if(false==diOld_enableRun2 && true==diNew_enableRun2|| false==initialedEnableRun2)
        {
            initialedEnableRun2=true;
            p_powerManger->setEnableRun(1,true);
            qDebug()<<"SafetyFunction::systemDiMonitor, ext di enable run2=enable";
            addMsg(ENUM_MSG_REMIND,"SafetyFunction","SafetyFunction",3704,-1,2);

        }
        else if(true==diOld_enableRun2 && false==diNew_enableRun2)
        {
            p_powerManger->programCease(userInfo,returnFlag);
            p_powerManger->setEnableRun(1,false);
            qDebug()<<"SafetyFunction::systemDiMonitor, ext di enable run2=disable";
            addMsg(ENUM_MSG_REMIND,"SafetyFunction","SafetyFunction",3705,-1,2);
        }
        diOld_enableRun2=diNew_enableRun2;
    }

    //enableRun3
    if(1==enableRun3Flag)
    {
        static bool diOld_enableRun3=false;
        bool diNew_enableRun3;
        diNew_enableRun3=p_powerManger->getSystemDiStatus(D_ENABLE_RUN3_IND);
        if(false==diOld_enableRun3 && true==diNew_enableRun3|| false==initialedEnableRun3)
        {
            initialedEnableRun3=true;
            p_powerManger->programCease(userInfo,returnFlag);
            p_powerManger->setEnableRun(2,true);
            qDebug()<<"SafetyFunction::systemDiMonitor, ext di enable run3=enable";
            addMsg(ENUM_MSG_REMIND,"SafetyFunction","SafetyFunction",3704,-1,3);

        }
        else if(true==diOld_enableRun3 && false==diNew_enableRun3)
        {
            p_powerManger->programCease(userInfo,returnFlag);
            p_powerManger->setEnableRun(2,false);
            qDebug()<<"SafetyFunction::systemDiMonitor, ext di enable run3=disable";
            addMsg(ENUM_MSG_REMIND,"SafetyFunction","SafetyFunction",3705,-1,3);
        }
        diOld_enableRun3=diNew_enableRun3;
    }

    //enableRun4
    if(1==enableRun3Flag)
    {
        static bool diOld_enableRun4=false;
        bool diNew_enableRun4;
        diNew_enableRun4=p_powerManger->getSystemDiStatus(D_ENABLE_RUN4_IND);
        if(false==diOld_enableRun4 && true==diNew_enableRun4|| false==initialedEnableRun4)
        {
            initialedEnableRun4=true;
            p_powerManger->setEnableRun(3,true);
            qDebug()<<"SafetyFunction::systemDiMonitor, ext di enable run4=enable";
            addMsg(ENUM_MSG_REMIND,"SafetyFunction","SafetyFunction",3704,-1,4);

        }
        else if(true==diOld_enableRun4 && false==diNew_enableRun4)
        {
            p_powerManger->programCease(userInfo,returnFlag);
            p_powerManger->setEnableRun(3,false);
            qDebug()<<"SafetyFunction::systemDiMonitor, ext di enable run4=disable";
            addMsg(ENUM_MSG_REMIND,"SafetyFunction","SafetyFunction",3705,-1,4);
        }
        diOld_enableRun4=diNew_enableRun4;
    }
}


int SafetyFunction::writeSettingFile(QString settingFile)
{
//    int flag = 1;
//    QString firstTimeFlagStr = SAFETY_SETTING_FILE_PATH;
//    QString strConfigGroup = "";
//    QString strConfigKey = "";

//    QSettings settings(firstTimeFlagStr,QSettings::IniFormat);
//    settings.setIniCodec("UTF-8");
//    strConfigGroup = "SystemInfo";
//    settings.beginGroup(strConfigGroup);

//    strConfigKey = "firstTimeFlag";
//    settings.setValue(strConfigKey,QVariant(flag));
//    settings.endGroup();

}

int SafetyFunction::readSettingFile(QString settingFile)
{

    qDebug()<<"SafetyFunction::readSettingFile,fileName="<<settingFile;
    QSettings settings(settingFile, QSettings::IniFormat);

    powerCheckFlag = settings.value("SystemInfo/powerCheckFlag").toInt();
    enableRun1Flag = settings.value("SystemInfo/enableRun1Flag").toInt();
    enableRun2Flag = settings.value("SystemInfo/enableRun2Flag").toInt();
    enableRun3Flag = settings.value("SystemInfo/enableRun3Flag").toInt();
    enableRun4Flag = settings.value("SystemInfo/enableRun4Flag").toInt();
    resetAlarmFlag = settings.value("SystemInfo/resetAlarmFlag").toInt();
    extServOnFlag = settings.value("SystemInfo/extServOnFlag").toInt();
    stopFlag = settings.value("SystemInfo/stopFlag").toInt();
    ceaseFlag = settings.value("SystemInfo/ceaseFlag").toInt();
    runFlag = settings.value("SystemInfo/runFlag").toInt();
    emergencyStopMonitorFlag = settings.value("SystemInfo/emergencyStopMonitorFlag").toInt();
    hmiServoDiMonitorFlag = settings.value("SystemInfo/hmiServoDiMonitorFlag").toInt();
    autoStartUpServOnFlag = settings.value("SystemInfo/autoStartUpServOnFlag").toInt();
    isEnableRun_autoMode = settings.value("SystemInfo/isEnableRun_autoMode").toInt();
    runSafetyCheck1Flag = settings.value("SystemInfo/runSafetyCheck1Flag").toInt();
    runSafetyCheck2Flag = settings.value("SystemInfo/runSafetyCheck2Flag").toInt();
    runSafetyCheck3Flag = settings.value("SystemInfo/runSafetyCheck3Flag").toInt();
    runSafetyCheck4Flag = settings.value("SystemInfo/runSafetyCheck4Flag").toInt();
    QString tmpStr;
    for(int i=0;i<D_SLOWDOWN_SIZE;i++)
    {
        tmpStr="SystemInfo/slowDownFlag";
        tmpStr+=QString::number(i+1);
        slowDownFlag[i] = settings.value(tmpStr).toInt();
        tmpStr="SystemInfo/slowDownRatio";
        tmpStr+=QString::number(i+1);
        slowDownRatio[i] = settings.value(tmpStr).toDouble();
        tmpStr="SystemInfo/slowDownTime";
        tmpStr+=QString::number(i+1);
        slowDownTime[i] = settings.value(tmpStr).toDouble();
    }

    ultrasonicCheckFlag=settings.value("SystemInfo/ultrasonicCheckFlag").toInt();
    isPlayErrorMusic=settings.value("SystemInfo/isPlayErrorMusic").toInt();
    isPlayBarrierMusic=settings.value("SystemInfo/isPlayBarrierMusic").toInt();

    QString strConfigKey;
    QVariant strConfigValue;
    QString strConfigGroup = "SystemInfo";
    settings.beginGroup(strConfigGroup);
    //laser1MonitorFlag
    strConfigKey = "laser1MonitorFlag";
    if(false==settings.contains(strConfigKey))
    {
        return -1;
    }
    strConfigValue = settings.value(strConfigKey);
    laser1MonitorFlag= strConfigValue.toInt();

    //laser2MonitorFlag
    strConfigKey = "laser2MonitorFlag";
    if(false==settings.contains(strConfigKey))
    {
        return -1;
    }
    strConfigValue = settings.value(strConfigKey);
    laser2MonitorFlag= strConfigValue.toInt();

    //laserOkValue
    strConfigKey = "laserOkValue";
    if(false==settings.contains(strConfigKey))
    {
        return -1;
    }
    strConfigValue = settings.value(strConfigKey);
    laserOkValue= strConfigValue.toBool();


    settings.endGroup();
    qDebug() << "SafetyFunction::readSettingFile()powerCheckFlag =\n"<<powerCheckFlag;

    return 1;
}

#include <unistd.h>
#include <sys/syscall.h>
#define gettid() syscall(__NR_gettid)

void* SafetyFunction::threadRun(void *)
{
    printf("SafetyFunction theread start run !!!");
#if 0
    int max_cpus = sysconf(_SC_NPROCESSORS_ONLN);
    printf("Max CPUs = %d\n", max_cpus);

    /* lock all memory (prevent swapping) */
    if (mlockall(MCL_CURRENT|MCL_FUTURE) == -1) {
        printf("printf: mlockall\n");
    }

    set_latency_target();
    set_cpu(0);
#endif

    struct sched_param param;
    param.__sched_priority = 50;
    sched_setscheduler(0,SCHED_FIFO,&param);
    qDebug()<<"************************SafetyFunction pid="<<gettid();
    usleep(659000);
    //setStackSize(81920);
    int policy, priority;
    pthread_getschedparam(pthread_self(), &policy, (struct sched_param *)&priority);
//    if(mlockall(MCL_CURRENT|MCL_FUTURE) == -1) {
//        qDebug("mlockall failed");
//    }
//    clock_gettime( CLOCK_MONOTONIC, &cycleTimespec );

   // qDebug()<<"this=="<<this;
   // pthread_setcancelstate(PTHREAD_CANCEL_ENABLE, NULL); //允许退出线程
    //pthread_setcanceltype(PTHREAD_CANCEL_ASYNCHRONOUS, NULL); //设置立即取消

    // 等待急停　di　模块初始化
    QThread::msleep(5000);//3000ms时，上海客户还是出现了１次急停报警。
    while(1)
    {
       // pthread_testcancel();/*the thread can be killed only here*/
         QThread::msleep(1000);

        while(true==safetyCheckFlag)
        {
//            QThread::msleep(D_SAFETY_LOOP_PERIOD);充电的时候没有响应
            realTimeSleep(D_SAFETY_LOOP_PERIOD);
            // 循环安全检查
            safetyCheckLoop();
        }
    }
    printf("safetyCheck thread quit!!!");
}

void* SafetyFunction::threadTask( void* classPtr )
{
    qDebug()<<"SafetyFunction thread run-------------------------";
    return ((SafetyFunction*)classPtr)->threadRun(NULL);
}

void SafetyFunction::createThread()
{
    int ret=pthread_create( &threadId, NULL, &SafetyFunction::threadTask,this);
    qDebug()<<"SafetyFunction createThread-------------------------";
}

void SafetyFunction::realTimeSleep(long long umicoroSecond)
{
    // 处理qt事件
//    QCoreApplication::processEvents();

    struct timespec time;
    clock_gettime(CLOCK_REALTIME,&time);
    time.tv_nsec += umicoroSecond * 1000;
    while(time.tv_nsec >= NSEC_PER_SEC)
    {
        time.tv_nsec -= NSEC_PER_SEC;
        ++time.tv_sec;
    }
    clock_nanosleep(CLOCK_REALTIME,TIMER_ABSTIME,&time,NULL);

}



void SafetyFunction::addMsg(int messageLevel, string componentName, string messageType,
                           int messageCode, int robotId,
                           int parameter1, int parameter2, int parameter3, int parameter4)
{
    Message tmpMsg;
    tmpMsg.MessageLevel = messageLevel;
    tmpMsg.componentClassName = componentName;
    tmpMsg.messageType = messageType;
    tmpMsg.robotId = robotId;
    tmpMsg.messageCode = messageCode;



    QString infomationStr;

    switch(messageCode)
    {
    case 3724:
    {
        infomationStr =  QObject::tr("外部ＤＩ信号启动了运动解释器(调度模式)！");
        break;
    }
    case 3723:
    {
        infomationStr =  QObject::tr("外部ＤＩ信号暂停了运动解释器(调度模式)！");
        break;
    }
    case 3722:
    {
        infomationStr =  QObject::tr("自动开启电机使能失败！");
        break;
    }
    case 3721:
    {
        infomationStr =  QObject::tr("安全模块初始化失败！");
        break;
    }
    case 3720:
    {
        infomationStr =  QObject::tr("安全模块读取配置文件失败！");
        break;
    }
    case 3719:
    {
        infomationStr =  QObject::tr("激光雷达（id号%1）输出故障电平！")
                .arg(parameter1);
        break;
    }
    case 3718:
    {
        infomationStr =  QObject::tr("超声波传感器（id号%1）通讯失败！")
                .arg(parameter1);
        break;
    }
    case 3717:
    {
        infomationStr =  QObject::tr("检测到有错误消息发生，强制关闭了电机！");
        break;
    }
    case 3716:
    {
        infomationStr =  QObject::tr("检测到有错误消息发生，强制复位了程序运行！");
        break;
    }
    case 3715:
    {
        infomationStr =  QObject::tr("超声波模块初始化失败！");
        break;
    }
    case 3714:
    {
        infomationStr =  QObject::tr("没有获取到急停开关信号，请检查ＩＯ模块！");
        break;
    }
    case 3713:
    {
        infomationStr =  QObject::tr("外部ＤＩ信号启动了程序(非调度模式)！");
        break;
    }
    case 3712:
    {
        infomationStr =  QObject::tr("外部ＤＩ信号暂停了程序(非调度模式)！");
        break;
    }
    case 3711:
    {
        infomationStr =  QObject::tr("外部ＤＩ信号停止了程序！");
        break;
    }
    case 3710:
    {
        infomationStr =  QObject::tr("外部ＤＩ信号复位了报警！");
        break;
    }
    case 3709:
    {
        infomationStr =  QObject::tr("外部ＤＩ信号关闭了使能！");
        break;
    }
    case 3708:
    {
        infomationStr =  QObject::tr("外部ＤＩ信号启动了使能！");
        break;
    }
    case 3707:
    {
        infomationStr =  QObject::tr("减速运行,ＤＩ信号(%3)取消了减速运行！(叠加速率百分数＝%1, 时间毫秒＝%2)")
                .arg(parameter1).arg(parameter2).arg(parameter3);
        break;
    }
    case 3706:
    {
        infomationStr =  QObject::tr("减速运行,ＤＩ信号(%3)设置了减速运行！(叠加速率百分数＝%1, 时间毫秒＝%2)")
                .arg(parameter1).arg(parameter2).arg(parameter3);
        break;
    }
    case 3705:
    {
        infomationStr =  QObject::tr("外部enablerun ＤＩ信号%1设置了 禁止程序运行！")
                .arg(parameter1);
        break;
    }
    case 3704:
    {
        infomationStr =  QObject::tr("外部enablerun ＤＩ信号%1设置了 允许程序运行！")
                .arg(parameter1);
        break;
    }
    case 3703:
    {
        infomationStr =  QObject::tr("检测到系统电源断电，%1秒后，系统将保护性关机！")
                .arg(parameter1);
        break;
    }
    case 3702:
    {
        infomationStr =  QObject::tr("检测到有错误消息发生，强制暂停了程序运行！");
        break;
    }
    case 3701:
    {
        infomationStr =  QObject::tr("急停没有手动软件清除，不允许使能！");
        break;
    }
    case 3700:
    {
        infomationStr =  QObject::tr("急停开关按下了！");
        break;
    }

        break;
    }


    tmpMsg.MessageInformation = infomationStr.toStdString();

    MessageLog::getInstance()->addMessage(tmpMsg);
}


